#include "ros2_gui_bridge/ros2_gui_bridge_node.hpp"
#include <unistd.h>  // 包含 close 函数声明
#include <hand_msgs/msg/motor_state.hpp>

std::mutex mtx;
std::vector<float> motor_ctrls_; // ROS2话题中的电机控制量
std::vector<float> sensor_dts_; // ROS2反馈话题中的传感器数据
std::vector<float> joint_ctrls_; // 新增：强化学习环境输出的控制量

int sockfd = -1;
rclcpp::Node::SharedPtr node;
volatile std::sig_atomic_t signal_received = 0;
size_t ctrls_count = 23;
size_t sensor_count = 15;
size_t joint_count = 23;         //用于记录关节数据数量
size_t motor_count = 18;         //用于记录电机数据数量
unsigned char mc[BUFFER_SIZE];  // 电机期盼数据反馈缓冲
unsigned char js[BUFFER_SIZE];  // 关节实际数据反馈缓冲
unsigned char sd[BUFFER_SIZE];  // 传感器数据反馈缓冲
unsigned char ms[BUFFER_SIZE];  // 电机状态数据反馈缓冲
unsigned char rl[BUFFER_SIZE];	// 强化学习控制缓冲
std::thread send_thread;

// 新增结构体数组定义
motor_t motor_[100];
joint_t joint_[100];
sensor_t sensor_[100];

// 将 float 类型数据转化成 unsigned char 类型数组的函数
void float2uchar(float value, unsigned char* buffer) {
    memcpy(buffer, &value, sizeof(float));
}

// unsigned char 类型数组转化成 float 类型数据的函数
float uchar2float(const unsigned char* buffer) {
    float result;
    memcpy(&result, buffer, sizeof(float));
    return result;
}

// 将 float 向量中的数据转换为十六进制填充到 unsigned char 数组
void convertFloatVectorToHex(const std::vector<float>& floatVector, unsigned char* charArray) {
    // 前 4 个字节固定为 0
    for (int i = 0; i < 4; ++i) {
        charArray[i] = 0;
    }
    size_t index = 4;
    for (float value : floatVector) {
        unsigned char temp[sizeof(float)];
        float2uchar(value, temp);
        for (size_t i = 0; i < sizeof(float); ++i) {
            if (index < BUFFER_SIZE - 4) {
                charArray[index++] = temp[i];
            } else {
                break;
            }
        }
    }
    // 剩余空间置为 0，最后四位用于存放校验和
    while (index < BUFFER_SIZE) {
        charArray[index++] = 0;
    }
    // 计算并添加校验和
    chksum(charArray, BUFFER_SIZE);
}

// 求累加和的函数，并将累加和以小端模式填充到数组的最后四位
void chksum(unsigned char* arr, size_t len) {
    unsigned int sum = 0;
    for (size_t i = 0; i < len - 4; ++i) {
        sum += arr[i];
    }
    // 小端模式填充到数组的最后四位
    arr[len - 4] = static_cast<unsigned char>(sum & 0xFF);
    arr[len - 3] = static_cast<unsigned char>((sum >> 8) & 0xFF);
    arr[len - 2] = static_cast<unsigned char>((sum >> 16) & 0xFF);
    arr[len - 1] = static_cast<unsigned char>((sum >> 24) & 0xFF);
}

// void joint_controllers_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg) {
//     joint_ctrls_.clear();
//     // 取前 joint_count 个数据
//     for (size_t i = 0; i < std::min(joint_count, msg->data.size()); ++i) {
//         joint_ctrls_.push_back(msg->data[i]);
//     }
// }
void joint_controllers_callback(const sensor_msgs::msg::JointState::SharedPtr msg) {
    joint_ctrls_.clear();
    joint_count = msg->position.size();
    // 取前 joint_count 个位置控制量
    for (size_t i = 0; i < joint_count; ++i) {
        joint_ctrls_.push_back(msg->position[i]);
    }
}

// void motor_controllers_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg) {
//     motor_ctrls_.clear();
//     ctrls_count = msg->data.size();
//     // RCLCPP_INFO(node->get_logger(), "Received %zu data points in motor_controllers:", ctrls_count);
//     for (float value : msg->data) {
//         motor_ctrls_.push_back(value);
//         // RCLCPP_INFO(node->get_logger(), "  %f", value);
//     }
// }

void joint_status_callback(const sensor_msgs::msg::JointState::SharedPtr msg) {
    joint_count = msg->position.size();
    // 填充 actPOS
    for (size_t i = 0; i < joint_count; ++i) {
        joint_[i].actPOS = msg->position[i];
    }
    // 填充 actVEL，以 joint_count 为上限
    for (size_t i = 0; i < joint_count; ++i) {
        joint_[i].actVEL = msg->velocity[i];
    }
    // 填充 actTOR，以 joint_count 为上限
    for (size_t i = 0; i < joint_count; ++i) {
        joint_[i].actTOR = msg->effort[i];
    }
    // RCLCPP_INFO(node->get_logger(), "Received %zu data points in joint_status:", joint_count);
}

void sensor_status_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg) {
    sensor_dts_.clear();
    sensor_count = msg->data.size();
    // RCLCPP_INFO(node->get_logger(), "Received %zu data points in sensor_status:", sensor_count);
    for (float value : msg->data) {
        sensor_dts_.push_back(value);
        // RCLCPP_INFO(node->get_logger(), "  %f", value);
    }
}

void motor_status_callback(const hand_msgs::msg::MotorState::SharedPtr msg) {
    motor_count = msg->name.size(); // 根据 name 数量填充 motor_count

    for (size_t i = 0; i < motor_count; ++i) {
        motor_[i].target_position = static_cast<float>(msg->target_position[i]);
        motor_[i].actual_position = static_cast<float>(msg->actual_position[i]);
        motor_[i].target_force = static_cast<float>(msg->target_force[i]);
        motor_[i].actual_force = static_cast<float>(msg->actual_force[i]);
        motor_[i].position_error = static_cast<float>(msg->position_error[i]);
        motor_[i].force_error = static_cast<float>(msg->force_error[i]);
        motor_[i].stator_current = static_cast<float>(msg->stator_current[i]);
        motor_[i].supply_voltage = static_cast<float>(msg->supply_voltage[i]);
        motor_[i].motor_temp = static_cast<float>(msg->motor_temp[i]);
        motor_[i].transistor_temp = static_cast<float>(msg->transistor_temp[i]);
    }
}

void send_messages(const std::string& target_ip, int target_port) {
    struct sockaddr_in targetAddr;
    memset(&targetAddr, 0, sizeof(targetAddr));
    targetAddr.sin_family = AF_INET;
    targetAddr.sin_port = htons(target_port);
    if (inet_pton(AF_INET, target_ip.c_str(), &targetAddr.sin_addr) <= 0) {
        RCLCPP_ERROR(node->get_logger(), "Invalid target IP address: %s", target_ip.c_str());
        return;
    }

    int state = 0;
    ssize_t sent_mc;
    ssize_t sent_ms;
    ssize_t sent_js;
    ssize_t sent_sd;
    ssize_t sent_rl;
    while (rclcpp::ok() && !signal_received) {
        switch (state) {
            case 0:
                // 清零 mc 缓冲区
                memset(mc, 0, sizeof(mc));
                // 填充 mc 缓冲区
                convertFloatVectorToHex(motor_ctrls_, mc);
                // 修改第一个和第二字节
                mc[0] = 0xff;
                mc[1] = 0x5a;
                // 将 case 编号+1存入第三位
                mc[2] = static_cast<unsigned char>(state + 1);
                // 将 ctrls_count 存入第四字节
                mc[3] = static_cast<unsigned char>(ctrls_count);
                // 重新计算校验和
                chksum(mc, sizeof(mc));
                sent_mc = sendto(sockfd, mc, sizeof(mc), 0, (struct sockaddr*)&targetAddr, sizeof(targetAddr));
                if (sent_mc == -1) {
                    RCLCPP_ERROR(node->get_logger(), "Error sending mc UDP data: %s", std::strerror(errno));
                } else {
                    // RCLCPP_INFO(node->get_logger(), "Sent %zd bytes of mc to %s:%d", sent_mc, target_ip.c_str(), target_port);
                }
                state = 1;
                break;
            case 1:
                // 清零 ms 缓冲区
                memset(ms, 0, sizeof(ms));
                // 填充 ms 缓冲区
                for (size_t i = 0; i < motor_count; ++i) {
                    unsigned char temp[sizeof(float)];
                    float2uchar(motor_[i].target_position, temp);
                    for (size_t j = 0; j < sizeof(float); ++j) {
                        ms[4 + i * sizeof(float) + j] = temp[j];
                    }
                }
                // 修改第一个和第二字节
                ms[0] = 0xff;
                ms[1] = 0x5a;
                // 将 case 编号+1存入第三位
                ms[2] = static_cast<unsigned char>(state + 1);
                // 将 motor_count 存入第四字节
                ms[3] = static_cast<unsigned char>(motor_count);
                chksum(ms, sizeof(ms));
                sent_ms = sendto(sockfd, ms, sizeof(ms), 0, (struct sockaddr*)&targetAddr, sizeof(targetAddr));
                if (sent_ms == -1) {
                    RCLCPP_ERROR(node->get_logger(), "Error sending ms UDP data: %s", std::strerror(errno));
                } else {
                    // RCLCPP_INFO(node->get_logger(), "Sent %zd bytes of mc to %s:%d", sent_mc, target_ip.c_str(), target_port);
                }
                state = 2;
                break;
            case 2:
                // 清零 ms 缓冲区
                memset(ms, 0, sizeof(ms));
                // 填充 ms 缓冲区
                for (size_t i = 0; i < motor_count; ++i) {
                    unsigned char temp[sizeof(float)];
                    float2uchar(motor_[i].actual_position, temp);
                    for (size_t j = 0; j < sizeof(float); ++j) {
                        ms[4 + i * sizeof(float) + j] = temp[j];
                    }
                }
                // 修改第一个和第二字节
                ms[0] = 0xff;
                ms[1] = 0x5a;
                // 将 case 编号+1存入第三位
                ms[2] = static_cast<unsigned char>(state + 1);
                // 将 motor_count 存入第四字节
                ms[3] = static_cast<unsigned char>(motor_count);
                chksum(ms, sizeof(ms));
                sent_ms = sendto(sockfd, ms, sizeof(ms), 0, (struct sockaddr*)&targetAddr, sizeof(targetAddr));
                if (sent_ms == -1) {
                    RCLCPP_ERROR(node->get_logger(), "Error sending ms UDP data: %s", std::strerror(errno));
                } else {
                    // RCLCPP_INFO(node->get_logger(), "Sent %zd bytes of ms to %s:%d", sent_ms, target_ip.c_str(), target_port);
                }
                state = 3;
                break;
            case 3:
                // 清零 ms 缓冲区
                memset(ms, 0, sizeof(ms));
                // 填充 ms 缓冲区
                for (size_t i = 0; i < motor_count; ++i) {
                    unsigned char temp[sizeof(float)];
                    float2uchar(motor_[i].target_force, temp);
                    for (size_t j = 0; j < sizeof(float); ++j) {
                        ms[4 + i * sizeof(float) + j] = temp[j];
                    }
                }
                // 修改第一个和第二字节
                ms[0] = 0xff;
                ms[1] = 0x5a;
                // 将 case 编号+1存入第三位
                ms[2] = static_cast<unsigned char>(state + 1);
                // 将 motor_count 存入第四字节
                ms[3] = static_cast<unsigned char>(motor_count);
                chksum(ms, sizeof(ms));
                sent_ms = sendto(sockfd, ms, sizeof(ms), 0, (struct sockaddr*)&targetAddr, sizeof(targetAddr));
                if (sent_ms == -1) {
                    RCLCPP_ERROR(node->get_logger(), "Error sending ms UDP data: %s", std::strerror(errno));
                } else {
                    // RCLCPP_INFO(node->get_logger(), "Sent %zd bytes of ms to %s:%d", sent_ms, target_ip.c_str(), target_port);
                }
                state = 4;
                break;
            case 4:
                // 清零 ms 缓冲区
                memset(ms, 0, sizeof(ms));
                // 填充 ms 缓冲区
                for (size_t i = 0; i < motor_count; ++i) {
                    unsigned char temp[sizeof(float)];
                    float2uchar(motor_[i].actual_force, temp);
                    for (size_t j = 0; j < sizeof(float); ++j) {
                        ms[4 + i * sizeof(float) + j] = temp[j];
                    }
                }
                // 修改第一个和第二字节
                ms[0] = 0xff;
                ms[1] = 0x5a;
                // 将 case 编号+1存入第三位
                ms[2] = static_cast<unsigned char>(state + 1);
                // 将 motor_count 存入第四字节
                ms[3] = static_cast<unsigned char>(motor_count);
                chksum(ms, sizeof(ms));
                sent_ms = sendto(sockfd, ms, sizeof(ms), 0, (struct sockaddr*)&targetAddr, sizeof(targetAddr));
                if (sent_ms == -1) {
                    RCLCPP_ERROR(node->get_logger(), "Error sending ms UDP data: %s", std::strerror(errno));
                } else {
                    // RCLCPP_INFO(node->get_logger(), "Sent %zd bytes of ms to %s:%d", sent_ms, target_ip.c_str(), target_port);
                }
                state = 5;
                break;
            case 5:
                // 清零 ms 缓冲区
                memset(ms, 0, sizeof(ms));
                // 填充 ms 缓冲区
                for (size_t i = 0; i < motor_count; ++i) {
                    unsigned char temp[sizeof(float)];
                    float2uchar(motor_[i].position_error, temp);
                    for (size_t j = 0; j < sizeof(float); ++j) {
                        ms[4 + i * sizeof(float) + j] = temp[j];
                    }
                }
                // 修改第一个和第二字节
                ms[0] = 0xff;
                ms[1] = 0x5a;
                // 将 case 编号+1存入第三位
                ms[2] = static_cast<unsigned char>(state + 1);
                // 将 motor_count 存入第四字节
                ms[3] = static_cast<unsigned char>(motor_count);
                chksum(ms, sizeof(ms));
                sent_ms = sendto(sockfd, ms, sizeof(ms), 0, (struct sockaddr*)&targetAddr, sizeof(targetAddr));
                if (sent_ms == -1) {
                    RCLCPP_ERROR(node->get_logger(), "Error sending ms UDP data: %s", std::strerror(errno));
                } else {
                    // RCLCPP_INFO(node->get_logger(), "Sent %zd bytes of ms to %s:%d", sent_ms, target_ip.c_str(), target_port);
                }
                state = 6;
                break;
            case 6:
                // 清零 ms 缓冲区
                memset(ms, 0, sizeof(ms));
                // 填充 ms 缓冲区
                for (size_t i = 0; i < motor_count; ++i) {
                    unsigned char temp[sizeof(float)];
                    float2uchar(motor_[i].force_error, temp);
                    for (size_t j = 0; j < sizeof(float); ++j) {
                        ms[4 + i * sizeof(float) + j] = temp[j];
                    }
                }
                // 修改第一个和第二字节
                ms[0] = 0xff;
                ms[1] = 0x5a;
                // 将 case 编号+1存入第三位
                ms[2] = static_cast<unsigned char>(state + 1);
                // 将 motor_count 存入第四字节
                ms[3] = static_cast<unsigned char>(motor_count);
                chksum(ms, sizeof(ms));
                sent_ms = sendto(sockfd, ms, sizeof(ms), 0, (struct sockaddr*)&targetAddr, sizeof(targetAddr));
                if (sent_ms == -1) {
                    RCLCPP_ERROR(node->get_logger(), "Error sending ms UDP data: %s", std::strerror(errno));
                } else {
                    // RCLCPP_INFO(node->get_logger(), "Sent %zd bytes of ms to %s:%d", sent_ms, target_ip.c_str(), target_port);
                }
                state = 7;
                break;
            case 7:
                // 清零 ms 缓冲区
                memset(ms, 0, sizeof(ms));
                // 填充 ms 缓冲区
                for (size_t i = 0; i < motor_count; ++i) {
                    unsigned char temp[sizeof(float)];
                    float2uchar(motor_[i].stator_current, temp);
                    for (size_t j = 0; j < sizeof(float); ++j) {
                        ms[4 + i * sizeof(float) + j] = temp[j];
                    }
                }
                // 修改第一个和第二字节
                ms[0] = 0xff;
                ms[1] = 0x5a;
                // 将 case 编号+1存入第三位
                ms[2] = static_cast<unsigned char>(state + 1);
                // 将 motor_count 存入第四字节
                ms[3] = static_cast<unsigned char>(motor_count);
                chksum(ms, sizeof(ms));
                sent_ms = sendto(sockfd, ms, sizeof(ms), 0, (struct sockaddr*)&targetAddr, sizeof(targetAddr));
                if (sent_ms == -1) {
                    RCLCPP_ERROR(node->get_logger(), "Error sending ms UDP data: %s", std::strerror(errno));
                } else {
                    // RCLCPP_INFO(node->get_logger(), "Sent %zd bytes of ms to %s:%d", sent_ms, target_ip.c_str(), target_port);
                }
                state = 8;
                break;
            case 8:
                // 清零 ms 缓冲区
                memset(ms, 0, sizeof(ms));
                // 填充 ms 缓冲区
                for (size_t i = 0; i < motor_count; ++i) {
                    unsigned char temp[sizeof(float)];
                    float2uchar(motor_[i].supply_voltage, temp);
                    for (size_t j = 0; j < sizeof(float); ++j) {
                        ms[4 + i * sizeof(float) + j] = temp[j];
                    }
                }
                // 修改第一个和第二字节
                ms[0] = 0xff;
                ms[1] = 0x5a;
                // 将 case 编号+1存入第三位
                ms[2] = static_cast<unsigned char>(state + 1);
                // 将 motor_count 存入第四字节
                ms[3] = static_cast<unsigned char>(motor_count);
                chksum(ms, sizeof(ms));
                sent_ms = sendto(sockfd, ms, sizeof(ms), 0, (struct sockaddr*)&targetAddr, sizeof(targetAddr));
                if (sent_ms == -1) {
                    RCLCPP_ERROR(node->get_logger(), "Error sending ms UDP data: %s", std::strerror(errno));
                } else {
                    // RCLCPP_INFO(node->get_logger(), "Sent %zd bytes of ms to %s:%d", sent_ms, target_ip.c_str(), target_port);
                }
                state = 9;
                break;
            case 9:
                // 清零 ms 缓冲区
                memset(ms, 0, sizeof(ms));
                // 填充 ms 缓冲区
                for (size_t i = 0; i < motor_count; ++i) {
                    unsigned char temp[sizeof(float)];
                    float2uchar(motor_[i].motor_temp, temp);
                    for (size_t j = 0; j < sizeof(float); ++j) {
                        ms[4 + i * sizeof(float) + j] = temp[j];
                    }
                }
                // 修改第一个和第二字节
                ms[0] = 0xff;
                ms[1] = 0x5a;
                // 将 case 编号+1存入第三位
                ms[2] = static_cast<unsigned char>(state + 1);
                // 将 motor_count 存入第四字节
                ms[3] = static_cast<unsigned char>(motor_count);
                chksum(ms, sizeof(ms));
                sent_ms = sendto(sockfd, ms, sizeof(ms), 0, (struct sockaddr*)&targetAddr, sizeof(targetAddr));
                if (sent_ms == -1) {
                    RCLCPP_ERROR(node->get_logger(), "Error sending ms UDP data: %s", std::strerror(errno));
                } else {
                    // RCLCPP_INFO(node->get_logger(), "Sent %zd bytes of ms to %s:%d", sent_ms, target_ip.c_str(), target_port);
                }
                state = 10;
                break;
            case 10:
                // 清零 ms 缓冲区
                memset(ms, 0, sizeof(ms));
                // 填充 ms 缓冲区
                for (size_t i = 0; i < motor_count; ++i) {
                    unsigned char temp[sizeof(float)];
                    float2uchar(motor_[i].transistor_temp, temp);
                    for (size_t j = 0; j < sizeof(float); ++j) {
                        ms[4 + i * sizeof(float) + j] = temp[j];
                    }
                }
                // 修改第一个和第二字节
                ms[0] = 0xff;
                ms[1] = 0x5a;
                // 将 case 编号+1存入第三位
                ms[2] = static_cast<unsigned char>(state + 1);
                // 将 motor_count 存入第四字节
                ms[3] = static_cast<unsigned char>(motor_count);
                chksum(ms, sizeof(ms));
                sent_ms = sendto(sockfd, ms, sizeof(ms), 0, (struct sockaddr*)&targetAddr, sizeof(targetAddr));
                if (sent_ms == -1) {
                    RCLCPP_ERROR(node->get_logger(), "Error sending ms UDP data: %s", std::strerror(errno));
                } else {
                    // RCLCPP_INFO(node->get_logger(), "Sent %zd bytes of ms to %s:%d", sent_ms, target_ip.c_str(), target_port);
                }
                state = 11;
                break;
            case 11:
                // 清零 js 缓冲区
                memset(js, 0, sizeof(js));
                // 填充 js 缓冲区
                for (size_t i = 0; i < motor_count; ++i) {
                    unsigned char temp[sizeof(float)];
                    float2uchar(joint_[i].actPOS, temp);
                    for (size_t j = 0; j < sizeof(float); ++j) {
                        js[4 + i * sizeof(float) + j] = temp[j];
                    }
                }
                // 修改第一个和第二字节
                js[0] = 0xff;
                js[1] = 0x5a;
                // 将 case 编号+1存入第三位
                js[2] = static_cast<unsigned char>(state + 1);
                // 将 motor_count 存入第四字节
                js[3] = static_cast<unsigned char>(joint_count);
                chksum(js, sizeof(js));
                sent_js = sendto(sockfd, js, sizeof(js), 0, (struct sockaddr*)&targetAddr, sizeof(targetAddr));
                if (sent_js == -1) {
                    RCLCPP_ERROR(node->get_logger(), "Error sending js UDP data: %s", std::strerror(errno));
                } else {
                    // RCLCPP_INFO(node->get_logger(), "Sent %zd bytes of js to %s:%d", sent_ms, target_ip.c_str(), target_port);
                }
                state = 12;
                break;
            case 12:
                // 清零 js 缓冲区
                memset(js, 0, sizeof(js));
                // 填充 js 缓冲区
                for (size_t i = 0; i < motor_count; ++i) {
                    unsigned char temp[sizeof(float)];
                    float2uchar(joint_[i].actVEL, temp);
                    for (size_t j = 0; j < sizeof(float); ++j) {
                        js[4 + i * sizeof(float) + j] = temp[j];
                    }
                }
                // 修改第一个和第二字节
                js[0] = 0xff;
                js[1] = 0x5a;
                // 将 case 编号+1存入第三位
                js[2] = static_cast<unsigned char>(state + 1);
                // 将 motor_count 存入第四字节
                js[3] = static_cast<unsigned char>(joint_count);
                chksum(js, sizeof(js));
                sent_js = sendto(sockfd, js, sizeof(js), 0, (struct sockaddr*)&targetAddr, sizeof(targetAddr));
                if (sent_js == -1) {
                    RCLCPP_ERROR(node->get_logger(), "Error sending js UDP data: %s", std::strerror(errno));
                } else {
                    // RCLCPP_INFO(node->get_logger(), "Sent %zd bytes of js to %s:%d", sent_ms, target_ip.c_str(), target_port);
                }
                state = 13;
                break;
            case 13:
                // 清零 js 缓冲区
                memset(js, 0, sizeof(js));
                // 填充 js 缓冲区
                for (size_t i = 0; i < motor_count; ++i) {
                    unsigned char temp[sizeof(float)];
                    float2uchar(joint_[i].actTOR, temp);
                    for (size_t j = 0; j < sizeof(float); ++j) {
                        js[4 + i * sizeof(float) + j] = temp[j];
                    }
                }
                // 修改第一个和第二字节
                js[0] = 0xff;
                js[1] = 0x5a;
                // 将 case 编号+1存入第三位
                js[2] = static_cast<unsigned char>(state + 1);
                // 将 motor_count 存入第四字节
                js[3] = static_cast<unsigned char>(joint_count);
                chksum(js, sizeof(js));
                sent_js = sendto(sockfd, js, sizeof(js), 0, (struct sockaddr*)&targetAddr, sizeof(targetAddr));
                if (sent_js == -1) {
                    RCLCPP_ERROR(node->get_logger(), "Error sending js UDP data: %s", std::strerror(errno));
                } else {
                    // RCLCPP_INFO(node->get_logger(), "Sent %zd bytes of js to %s:%d", sent_ms, target_ip.c_str(), target_port);
                }
                state = 14;
                break;
            case 14:
                // 清零 sd 缓冲区
                memset(sd, 0, sizeof(sd));
                // 填充 sd 缓冲区
                convertFloatVectorToHex(sensor_dts_, sd);
                // 修改第一个和第二字节
                sd[0] = 0xff;
                sd[1] = 0x5a;
                // 将 case 编号+1存入第三位
                sd[2] = static_cast<unsigned char>(state + 1);
                // 将 15 存入第四字节
                sd[3] = 15;
                // 重新计算校验和
                chksum(sd, sizeof(sd));
                sent_sd = sendto(sockfd, sd, sizeof(sd), 0, (struct sockaddr*)&targetAddr, sizeof(targetAddr));
                if (sent_sd == -1) {
                    RCLCPP_ERROR(node->get_logger(), "Error sending sd UDP data: %s", std::strerror(errno));
                } else {
                    // RCLCPP_INFO(node->get_logger(), "Sent %zd bytes of sd to %s:%d", sent_sd, target_ip.c_str(), target_port);
                }
                state = 15;
                break;
            case 15:
                // 清零 rl 缓冲区
                memset(rl, 0, sizeof(rl));
                // 填充 rl 缓冲区
                convertFloatVectorToHex(joint_ctrls_, rl);
                // 修改第一个和第二字节
                rl[0] = 0xff;
                rl[1] = 0x5a;
                // 将 case 编号+1存入第三位
                rl[2] = static_cast<unsigned char>(state + 1);
                // 将 joint_count 存入第四字节
                rl[3] = static_cast<unsigned char>(joint_count);
                // 重新计算校验和
                chksum(rl, sizeof(rl));
                sent_rl = sendto(sockfd, rl, sizeof(rl), 0, (struct sockaddr*)&targetAddr, sizeof(targetAddr));
                if (sent_rl == -1) {
                    RCLCPP_ERROR(node->get_logger(), "Error sending rl UDP data: %s", std::strerror(errno));
                } else {
                    // RCLCPP_INFO(node->get_logger(), "Sent %zd bytes of rl to %s:%d", sent_rl, target_ip.c_str(), target_port);
                }
                state = 0;
                break;
        }
        std::this_thread::sleep_for(std::chrono::milliseconds(10));
    }
}    

void signal_handler(int signum) {
    signal_received = signum;
    RCLCPP_INFO(node->get_logger(), "Received signal %d. Shutting down...", signum);
    rclcpp::shutdown();
    if (send_thread.joinable()) {
        send_thread.join();
    }
}

int main(int argc, char * argv[]) {
    rclcpp::init(argc, argv);
    node = rclcpp::Node::make_shared("ros2_gui_bridge_node");

    // 注册信号处理函数
    std::signal(SIGINT, signal_handler);
    std::signal(SIGTERM, signal_handler);

    // auto motor_controllers_sub = node->create_subscription<std_msgs::msg::Float32MultiArray>(
    //     "motor_controllers", 10, motor_controllers_callback);
    auto joint_status_sub = node->create_subscription<sensor_msgs::msg::JointState>(
        "joint_status", 10, joint_status_callback);
    auto sensor_status_sub = node->create_subscription<std_msgs::msg::Float32MultiArray>(
        "sensor_status", 10, sensor_status_callback);
    auto motor_status_sub = node->create_subscription<hand_msgs::msg::MotorState>(
        "/motor_status", 10, motor_status_callback);
    // 新增：订阅 joint_controllers 话题
    // auto joint_controllers_sub = node->create_subscription<std_msgs::msg::Float32MultiArray>(
    //     "joint_controllers", 10, joint_controllers_callback);
    auto joint_controllers_sub = node->create_subscription<sensor_msgs::msg::JointState>(
        "joint_controllers", 10, joint_controllers_callback);

    // 创建 UDP 套接字
    sockfd = socket(AF_INET, SOCK_DGRAM, 0);
    if (sockfd == -1) {
        RCLCPP_ERROR(node->get_logger(), "Error creating UDP socket: %s", std::strerror(errno));
        return 1;
    }

    // 设置 SO_REUSEADDR 和 SO_REUSEPORT 选项
    int optval = 1;
    if (setsockopt(sockfd, SOL_SOCKET, SO_REUSEADDR, &optval, sizeof(optval)) == -1) {
        RCLCPP_ERROR(node->get_logger(), "Error setting socket option SO_REUSEADDR: %s", std::strerror(errno));
        close(sockfd);
        return 1;
    }
    if (setsockopt(sockfd, SOL_SOCKET, SO_REUSEPORT, &optval, sizeof(optval)) == -1) {
        RCLCPP_ERROR(node->get_logger(), "Error setting socket option SO_REUSEPORT: %s", std::strerror(errno));
        close(sockfd);
        return 1;
    }

    // 从 YAML 文件读取目标地址和本地端口
    std::string yaml_file_path = "/home/neousys/Works/ros2_ws/src/ros2_gui_bridge/config/config.yaml";
    try {
        YAML::Node config = YAML::LoadFile(yaml_file_path);
        std::string target_ip = config["target_ip"].as<std::string>();
        int target_port = config["target_port"].as<int>();

        send_thread = std::thread(send_messages, target_ip, target_port);

        try {
            rclcpp::spin(node);
        } catch (const std::exception& e) {
            RCLCPP_ERROR(node->get_logger(), "Exception in rclcpp::spin: %s", e.what());
        }

        rclcpp::shutdown();
        if (send_thread.joinable()) {
            send_thread.join();
        }
    } catch (const YAML::Exception& e) {
        RCLCPP_ERROR(node->get_logger(), "Error loading YAML file: %s", e.what());
    }

    if (close(sockfd) == -1) {
        RCLCPP_ERROR(node->get_logger(), "Error closing UDP socket: %s", std::strerror(errno));
    }
    return 0;
}    